#include "sensors.h"
#include "config.h"
#include "my_u8g2.h"
#include "plog.h"
#include <Wire.h>
#include <Arduino.h>

#include "Adafruit_SGP30.h"
#include <Adafruit_AHTX0.h>
#include <Adafruit_BMP280.h>

#ifdef SENSOR_SGP30_SENSIRION_DRIVER
#include "sensirion_sgp30/sensirion_common.h"
#include "sensirion_sgp30/sgp30.h"
#endif

Adafruit_SGP30 sgp30;

void GS_init()
{
#ifdef SENSOR_SGP30_SENSIRION_DRIVER
  while (sgp_probe() != STATUS_OK)
  {
    LOG_ERROR("SGP sensor probing failed, retrying...");
    U8G2_message("can't find SGP");
    delay(1000);
  }

  u16 scaled_ethanol_signal, scaled_h2_signal;
  s16 err = sgp_measure_signals_blocking_read(&scaled_ethanol_signal,
                                              &scaled_h2_signal);
  if (err == STATUS_OK)
    LOG_DEBUG("Ethanol signal: %f\tH2 signal: %u.%09llu", scaled_ethanol_signal / 512.0f, scaled_h2_signal >> 9,
              ((scaled_h2_signal & 0x01ff) * (u64)1000000000) >> 9);
  else
    LOG_ERROR("error reading signals");

  err = sgp_iaq_init();
  if (err != STATUS_OK)
    LOG_ERROR("sgp iaq init fail!");

  LOG_INFO("SGP30 found");
#else
  // adafruit库发热问题貌似无法解决，芯片特性，只要调用了IAQinit就开始加热。
  // 官方库可以稍微改善这个问题，至少在初始化的时候没有发热，但是采集数据时发热还是有。应该是芯片自身特性，因为看手册内部有heater controller
  while (!sgp30.begin(&Wire, false)) // 后续采集数据时再初始化IAQinit
  {
    LOG_ERROR("SGP30 Sensor not found :(, retrying...");
    U8G2_message("can't find SGP");
    delay(1000);
  }

  LOG_INFO("SGP30 found, serial #%02x%02x%02x",
           sgp30.serialnumber[0],
           sgp30.serialnumber[1],
           sgp30.serialnumber[2]);
#endif
}

void GS_start()
{
  while (!sgp30.IAQinit())
  {
    LOG_ERROR("SGP30 Sensor start failed!");
    U8G2_message("SGP start fail");
    delay(1000);
  }
  LOG_INFO("SGP30 started");
}

void GS_stop()
{
  // while (!sgp30.softReset())
  while (!GS_SGP30_soft_reset())
  {
    LOG_ERROR("SGP30 Sensor stopped failed!");
    U8G2_message("SGP stopped fail");
    delay(500);
  }
  LOG_INFO("SGP30 Sensor stopped");
}

// 原库的softReset有bug，把指令变成了0x0006 2个字节，底层addr地址并没有改变（还是0x58的设备地址，并不是0x00广播地址）。
boolean GS_SGP30_soft_reset()
{
  Wire.beginTransmission(0x00); // General Call地址

  // 发送复位命令 0x06
  Wire.write(0x06);

  uint8_t result = Wire.endTransmission();

  // 等待复位完成（SGP30需要10ms初始化）
  delay(10);

  return (result == 0);
}

// -----------------------------------
// RH → AH 转换（精度±1.5%）
// 输入：tempC(℃), relHumidity(%)
// 输出：绝对湿度(g/m³)
// -----------------------------------
float convertRHtoAH(float tempC, float relHumidity)
{
  // 饱和水汽压计算（Wexler公式）
  const float c1 = 6.112;
  const float c2 = 17.67;
  const float c3 = 243.5;
  float expArg = (c2 * tempC) / (tempC + c3);
  float p_sat = c1 * exp(expArg); // 单位：hPa

  // 实际水汽压
  float p_v = (p_sat * relHumidity) / 100.0;

  // 绝对湿度转换（理想气体定律）
  float t_kelvin = tempC + 273.15;
  return (216.7f * p_v) / t_kelvin;
}

GS_result_t GS_get_result(ES_result_t *es_res)
{
#ifdef ES_ENABLE
  // // 温度和湿度 补偿算法
  float tempC = es_res->temperature;    // 单位：摄氏度
  float relHumidity = es_res->humidity; // 单位：%
  float absHumidity = convertRHtoAH(tempC, relHumidity);
#endif

  GS_result_t ret = {};
#ifdef SENSOR_SGP30_SENSIRION_DRIVER
  u16 tvoc_ppb, co2_eq_ppm;
  s16 err = sgp_measure_iaq_blocking_read(&tvoc_ppb, &co2_eq_ppm);
  if (err == STATUS_OK)
  {
#ifdef SENSOR_RAW_DATA_PRINT
    Serial.printf("tVOC  Concentration: %dppb\n", tvoc_ppb);
    Serial.printf("CO2eq Concentration: %dppm\n", co2_eq_ppm);
#endif
  }
  else
    LOG_ERROR("error reading IAQ values");
#ifdef ES_ENABLE
  u32 ah = absHumidity * 1000; // absolute humidity in mg/m^3
  sgp_set_absolute_humidity(ah);
#endif

  ret.tvoc = tvoc_ppb;
  ret.eco2 = co2_eq_ppm;
#else
#ifdef ES_ENABLE
  if (!sgp30.setHumidity(absHumidity))
  {
    LOG_ERROR("setHumidity failed");
  }
#endif
  if (!sgp30.IAQmeasure())
  {
    LOG_ERROR("Measurement failed");
    return ret;
  }
#ifdef SENSOR_RAW_DATA_PRINT
  Serial.print("[sgp30]  - raw data: eCO2: ");
  Serial.print(sgp30.eCO2);
  Serial.print(" ppm\t");
  Serial.print("TVOC: ");
  Serial.print(sgp30.TVOC);
  Serial.println(" ppb");
#endif
  ret.tvoc = sgp30.TVOC;
  ret.eco2 = sgp30.eCO2;
#endif

  return ret;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Adafruit_AHTX0 aht;
Adafruit_BMP280 bmp; // I2C

void ES_init()
{
  while (!aht.begin())
  {
    LOG_ERROR("Could not find AHT? Check wiring");
    U8G2_message("can't find AHT");
    delay(1000);
  }
  LOG_INFO("AHT20 found");

  while (!bmp.begin())
  {
    LOG_ERROR("Could not find BMP? Check wiring");
    U8G2_message("can't find BMP");
    delay(1000);
  }
  LOG_INFO("BMP280 found, sensorID: %x", bmp.sensorID()); //   ID of 0x56-0x58 represents a BMP 280,
  bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,           /* Operating Mode. */
                  Adafruit_BMP280::SAMPLING_X2,           /* Temp. oversampling */
                  Adafruit_BMP280::SAMPLING_X16,          /* Pressure oversampling */
                  Adafruit_BMP280::FILTER_X16,            /* Filtering. */
                  Adafruit_BMP280::STANDBY_MS_500);       /* Standby time. */
}

ES_result_t ES_get_result()
{
  ES_result_t result;
  // aht20
  sensors_event_t aht20_humidity, aht20_temp;
  aht.getEvent(&aht20_humidity, &aht20_temp); // populate temp and humidity objects with fresh data
#ifdef SENSOR_RAW_DATA_PRINT
  Serial.print("[aht20]  - raw data: Temperature: ");
  Serial.print(aht20_temp.temperature);
  Serial.print(" *C\t");
  Serial.print("Humidity: ");
  Serial.print(aht20_humidity.relative_humidity);
  Serial.println("% rH");
#endif
  // bmp280
  float bmp280_temp, bmp280_pressure, bmp280_altitude;
  bmp280_temp = bmp.readTemperature();
  bmp280_pressure = bmp.readPressure();
  bmp280_altitude = bmp.readAltitude(1013.25); /* Adjusted to local forecast! */
#ifdef SENSOR_RAW_DATA_PRINT
  Serial.print(F("[bmp280] - raw data: Temperature: "));
  Serial.print(bmp280_temp);
  Serial.print(" *C\t");

  Serial.print(F("Pressure: "));
  Serial.print(bmp280_pressure);
  Serial.print(" Pa\t");

  Serial.print(F("Approx altitude: "));
  Serial.print(bmp280_altitude);
  Serial.println(" m");
#endif

  // 温度使用简单的加权平均算法
  result.temperature = (aht20_temp.temperature + bmp280_temp) / float(2);
  result.humidity = aht20_humidity.relative_humidity;
  result.pressure = bmp280_pressure;
  return result;
}

Sensor_result_t Sensor_collect_once()
{
  Sensor_result_t ret = {};
#ifdef ES_ENABLE
  ret.es = ES_get_result();
  ret.gs = GS_get_result(&ret.es);
#else
  ret.gs = GS_get_result(&ret.es);
#endif
  return ret;
}

#define QUEUE_SIZE 60
// 循环队列结构
static struct
{
  Sensor_result_t data[QUEUE_SIZE];
  uint16_t head = 0;  // 最老数据索引
  uint16_t tail = 0;  // 指向下一个最新数据索引
  uint16_t count = 0; // 当前数据量
} sensorQueue;
// deprecated
// 生产者函数：采集数据 (高频调用)
void Sensor_collect_loop()
{
  Sensor_result_t new_data = Sensor_collect_once();
  // 写入新数据到队尾
  sensorQueue.data[sensorQueue.tail] = new_data;

  // 更新队尾指针
  sensorQueue.tail = (sensorQueue.tail + 1) % QUEUE_SIZE;

  if (sensorQueue.count < QUEUE_SIZE)
  {
    sensorQueue.count++;
  }
  else
  {
    // 队列已满，覆盖最老数据
    sensorQueue.head = (sensorQueue.head + 1) % QUEUE_SIZE;
  }
}
// deprecated
// 消费者函数：获取均值结果 (低频调用)
void Sensor_get_result(Sensor_result_t *result)
{
  // 临时累加器
  uint32_t tvoc_sum = 0;
  uint32_t eco2_sum = 0;
  float pressure_sum = 0.0f;
  float temp_sum = 0.0f;
  float humidity_sum = 0.0f;

  // 处理空队列情况
  if (sensorQueue.count == 0)
  {
    *result = (Sensor_result_t){
        .gs = {0, 0},
        .es = {0.0f, 0.0f, 0.0f}};
    return;
  }

  // 遍历队列计算总和
  uint16_t index = sensorQueue.head;
  for (int i = 0; i < sensorQueue.count; i++)
  {
    tvoc_sum += sensorQueue.data[index].gs.tvoc;
    eco2_sum += sensorQueue.data[index].gs.eco2;
    pressure_sum += sensorQueue.data[index].es.pressure;
    temp_sum += sensorQueue.data[index].es.temperature;
    humidity_sum += sensorQueue.data[index].es.humidity;

    index = (index + 1) % QUEUE_SIZE;
  }

  // 计算均值
  result->gs.tvoc = (uint16_t)(tvoc_sum / sensorQueue.count);
  result->gs.eco2 = (uint16_t)(eco2_sum / sensorQueue.count);
  result->es.pressure = pressure_sum / float(sensorQueue.count);
  result->es.temperature = temp_sum / float(sensorQueue.count);
  result->es.humidity = humidity_sum / float(sensorQueue.count);
}

void wait_until_data_stable()
{
  Sensor_result_t data = {};
  while (1)
  {
    Sensor_result_t data = Sensor_collect_once();
    if (data.gs.tvoc != 0 || data.gs.eco2 != 400)
      return;
    else
    {
      LOG_INFO("waiting tvoc & eco2 data stable...");
      // U8G2_message("sgp waking up");
    }
    delay(1000);
  }
}
